package rbb.itm5.bwinf.roboter.Control.Drivers;

import java.awt.Point;
import java.awt.Rectangle;
import java.util.ArrayList;
import rbb.itm5.bwinf.roboter.Control.DriverInterface;
import rbb.itm5.bwinf.roboter.Control.Roboter;
import rbb.itm5.bwinf.roboter.Model.Obstacle;
import rbb.itm5.bwinf.roboter.Model.Step;
/**
 * Dies ist mein Steuerungsprogramm, welches den Roboter in Ziel bewegt.
 * @author Dominik Jakielski
 */
public class Jakielski extends DriverInterface{
    private Rectangle room=new Rectangle(0, 0, 1, 1);
    private int strateOn=0,counter=0,weg=4;
    private int MinimumX=0,MaximumX=0,MinimumY=0,MaximumY=0;
    
    public Jakielski(Roboter robot) {
            super(robot);            
    }
    @Override
    public String getName() {
            return "Jakielski";
    }
    @Override
    protected void onStart() {
        paintObstacles=true;
        paintGoneSteps=true;
        obstacles.clear();
        strateOn=0;
        counter=0;
        room=new Rectangle(0, 0, 1, 1);
        MinimumX=0;
        MinimumY=0;
        MaximumX=0;
        MaximumY=0;
        if(!robot.hasColision()){
            int anz=(int) (Math.random()*4);
            for (int i = 0; i < anz; i++) {
                turn();
            }
        }
        while (robot.hasColision()) {            
            turn();
        }
    }

    @Override
    protected void drive() throws InterruptedException {
        int countLeft=0,countRight=0;
        boolean forward=false,left=false,right=false;
        do{
            boolean oldLeft=left,oldRight=right;
            forward=robot.hasColision();
            left=robot.hasLeftColision();
            right=robot.hasRightColision();

            if(forward) registerObstacle(robot.getDirection());
            if(left)registerObstacle(robot.getDirectionLeft());
            if(right)registerObstacle(robot.getDirectionRight());


            //Prüfe und rechne neue Raumgröße
            checkRoomSize();
            //Richtungswechsel berechnet bzw. steuern
            Obstacle ob=findObstacleOnPosition(robot.getDirection(), robot.getPostion());     
            float turnLeft=0,turnRight=0,goForward=0;
            if(ob!=null){
                //Analyse des Hinderniss
            }
            if(oldLeft && !left)
                turnLeft+=0.8; 
            else if(!left){
                turnLeft+=0.2;
            }
            if(!left && isRobotOnWall(robot.getDirectionLeft())) turnLeft+=2.4;
            
            if(oldRight && !right) 
                turnRight+=0.8;
            else if(!right){
                turnRight+=0.2;
            }
            if(!right && isRobotOnWall(robot.getDirectionRight())) turnRight+=2.4;
            int wasThereLeft=wasThere(robot.getNextPosition(robot.getDirectionLeft())),wasThereRight=wasThere(robot.getNextPosition(robot.getDirectionRight())),wasThere=wasThere(robot.getNextPosition());
            

            //if(wasThereLeft==0) turnLeft+=0.4; else turnLeft-=wasThereLeft*0.002;
            //if(wasThereRight==0) turnRight+=0.4; else turnRight-=wasThereRight*0.002;
            if(!forward && wasThere==0) goForward+=1.6; else goForward-=wasThere*0.003;

            
            if(!forward){
                if(wasThere(robot.getNextPosition())>0) goForward+=0.5; else goForward+=0.8;
                if(countLeft>=3 || countRight>=3) goForward+=1;
                
            } else goForward-=0.1;
            
            
            //System.out.println(goForward+" - "+turnLeft+" - "+turnRight);
            //System.out.println("("+isRobotOnWall()+")   Forward="+isRobotOnWall(robot.getDirection())+" - Left="+isRobotOnWall(robot.getDirectionLeft())+" - Right="+isRobotOnWall(robot.getDirectionRight()));
            
            if(goForward>Math.max(turnLeft, turnRight) || (goForward==Math.max(turnLeft, turnRight) && Math.random()>=0.5)){
                robot.goForward();
                countLeft=0;
                countRight=0;
                strateOn++;
            }else if(turnLeft==turnRight){
                if(forward)turn();
                robot.goForward();
            }else if(turnLeft>turnRight){
                robot.turnLeft();
                if(!left) robot.goForward();
                countLeft++;
                strateOn=0;
            }else{
                robot.turnRight();
                if(!right) robot.goForward();
                countRight++;
                strateOn=0;
            }
            
            waitForNextStep();
        }while(!robot.checkGoalHasFound());
    }
    private void registerObstacle(int direction) {
            Point p=robot.getPostion();
            switch (direction) {
            case Roboter.DIRECTION_UP:
                    p.y--;
                    break;
            case Roboter.DIRECTION_LEFT:
                    p.x--;
                    break;
            case Roboter.DIRECTION_DOWN:
                    p.y++;
                    break;
            case Roboter.DIRECTION_RIGHT:
                    p.x++;
                    break;
            }
            Obstacle ob=findObstacleOnPosition(p.x,p.y);
            if(ob==null){
                    //System.out.println("Hinderniss "+Roboter.DIRECTION[direction]+" auf ("+p.x+","+p.y+")");
                    boolean onWall=false;
                    if(p.x==room.x-1 || p.x==room.width-room.x || (room.x<0 && p.x==room.width+room.x)) onWall=true;
                    if(p.y==room.y-1 || p.y==room.height-room.y|| (room.y<0 && p.y==room.height+room.y)) onWall=true;
                    if(onWall){
                            obstacles.add(new Obstacle(p.x, p.y, 1, 1,true));
                    }else
                            obstacles.add(new Obstacle(p.x, p.y, 1, 1));
            }
    }
    private Obstacle findObstacleOnPosition(int direction,Point p) {
            switch (direction) {
            case Roboter.DIRECTION_UP:
                    p.y--;
                    break;
            case Roboter.DIRECTION_LEFT:
                    p.x--;
                    break;
            case Roboter.DIRECTION_DOWN:
                    p.y++;
                    break;
            case Roboter.DIRECTION_RIGHT:
                    p.x++;
                    break;
            }
            return findObstacleOnPosition(p.x,p.y);
    }
    private Obstacle findObstacleOnPosition(int x, int y) {
            Obstacle re=null;
            mergeObstacles();
            for (Obstacle ob : obstacles) {
                    if(ob.getPostion().x==x && ob.getPostion().y==y){
                            re=ob;
                            break;
                    }
                    if(x>ob.getPostion().x && x>ob.getPostion().y && x<=ob.getPostion().x+ob.getSize().width-1 && y<=ob.getPostion().y+ob.getSize().height-1){
                            re=ob;
                            break;
                    }
            }
            return re;
    }
    private void mergeObstacles() {

    }
    private void RoomSizeChanged(Rectangle oldRoom) {
            //System.out.println("x="+room.x+"  y="+room.y+"  w="+room.width+"  h="+room.width);
            //Alte Wände enfternen
            int remove=0;
            boolean onWall=true;
            for (Obstacle ob : obstacles) {
                    onWall=false;
                    if(counter==4 && strateOn==1 && ob.getPostion().y==1){
                            System.out.println("Fehler");
                    }
                    if(ob.getPostion().x==room.x-1 || ob.getPostion().x+ob.getSize().width-1==room.width-room.x || (room.x<0 && ob.getPostion().x+ob.getSize().width-1==room.width+room.x)) onWall=true;
                    if(ob.getPostion().y==room.y-1 || ob.getPostion().y+ob.getSize().height-1==room.height-room.y|| (room.y<0 && ob.getPostion().y+ob.getSize().height-1==room.height+room.y)) onWall=true;
                    if(!onWall && ob.isOnTheWall()){
                            remove++;
                    }
                    ob.setEditable(true);
                    ob.setOnTheWall(onWall);
            }
            if(remove>0) System.out.println(remove+" Wandhindernisse wurden entfernt");
    }
    private void turn() {
            boolean left=robot.hasLeftColision(),right=robot.hasRightColision();
            if(left&& right){
                robot.turnLeft();
                robot.turnLeft();
            }else if(!left && !right){
                    if(Math.random()>=0.5) 
                            robot.turnLeft();
                    else 
                            robot.turnRight();
            }else if(!left)
                    robot.turnLeft();
            else if(!right)
                    robot.turnRight();
            strateOn=0;
    }
    public boolean isRobotOnWall() {
        boolean onWall=false;
	Point p=robot.getPostion();
        if(p.x==room.x-1 || p.x==room.width-room.x || (room.x<0 && p.x==room.width+room.x)) onWall=true;
	if(p.y==room.y-1 || p.y==room.height-room.y|| (room.y<0 && p.y==room.height+room.y)) onWall=true;
        return onWall;
    }
    public boolean isRobotOnWall(int Direction) {
        boolean onWall=false;
        Point p=robot.getPostion();
	switch(Direction){
            case Roboter.DIRECTION_LEFT:
                onWall=p.x==room.x;
                break;
            case Roboter.DIRECTION_UP:
                onWall=p.y==room.y;
                break;
            case Roboter.DIRECTION_RIGHT:
                onWall=p.x==room.width+room.x-1;
                break;
            case Roboter.DIRECTION_DOWN:
                onWall=p.y==room.height+room.y-1;
                break;
            
        }
        return onWall;
    }
    private void hardWay() throws InterruptedException{
        switch (weg) {
            case 1:
                    if(counter==0 && strateOn==7){
                            robot.turnRight();
                            strateOn=0;
                            counter++;
                    }
                    if(counter==1 && strateOn==11){
                            robot.turnLeft();
                            strateOn=0;
                            counter++;
                    }
                    if(counter==2 && strateOn==3){
                            strateOn=0;
                            throw new InterruptedException("Stopp");
                    }
                    break;
            case 2:
                    if(counter==0 && strateOn==7){
                            robot.turnRight();
                            strateOn=0;
                            counter++;
                    }
                    if(counter==1 && strateOn==12){
                            robot.turnRight();
                            strateOn=0;
                            counter++;
                    }
                    if(counter==2 && strateOn==12){
                            robot.turnRight();
                            strateOn=0;
                            counter++;
                    }
                    if(counter==3 && strateOn==10){
                            robot.turnLeft();
                            strateOn=0;
                            counter++;
                    }
                    if(counter==4 && strateOn==2){
                            strateOn=0;
                            robot.turnRight();
                            counter++;
                    }
                    if(counter==5 && strateOn==2){
                            strateOn=0;
                            throw new InterruptedException("Stopp");
                    }
                    break;
            }
    }
    private void checkRoomSize() {
        Point p=robot.getPostion();
        boolean MinMaxChanged=false;
        if(p.x<MinimumX){MinimumX=p.x;MinMaxChanged=true;}
        if(p.x>MaximumX){MaximumX=p.x;MinMaxChanged=true;}
        if(p.y<MinimumY){MinimumY=p.y;MinMaxChanged=true;}
        if(p.y>MaximumY){MaximumY=p.y;MinMaxChanged=true;}
        if(MinMaxChanged){
            Rectangle oldRoom=new Rectangle(room);
            room.setLocation(MinimumX, MinimumY);
            room.setSize(Math.abs(MinimumX)+MaximumX+1, Math.abs(MinimumY)+MaximumY+1);
            RoomSizeChanged(oldRoom);
        }
    }
    private void turnToDirection(int Direction){
        boolean turnLeft=robot.getDirection()<2;
        while (Direction!=robot.getDirection()) {            
            if (turnLeft)robot.turnLeft(); else robot.turnRight();
        }
    }
    
    private int wasThere(Point position){
        int wasThere=0;
        ArrayList<Step> steps = robot.getSteps();
        for (Step step : steps) {
            if(step.getX()==position.x && step.getY()==position.y){
                wasThere++;               
            }
        }
        return wasThere;
    }
    
    private int rnd(int von,int bis){
        return (int)((Math.random()*(Math.abs(bis-von)))+von);
    }

    
}